// =====================================================
// ====               MPU6050                       ====
// =====================================================
unsigned char Re_buf[11],counter=0;
unsigned char sign=0;
float a[3],w[3],Angle[3],T;
short sAccelerat[3],sAngleVelocity[3],sAngle[3],sT;
//======================================================
//==                  Motor programer                ===
//======================================================
//MOTOR(int (+/-)Left_PWM,int (+/-)Right_PWM)
//‘+’电机正转；‘—’电机反转
//PWM:范围0~255
//======================================================
int M_left1 =7;
int M_left2 =6;
int M_right1 =5;
int M_right2 =4;
 // =========================================================
// ===                    INTERRUPT                       ===
// ==========================================================
//External Interrupts: 2 (interrupt 0), 3 (interrupt 1)
//                     18 (interrupt 5),19 (interrupt 4)
//                     20 (interrupt 3), 21 (interrupt 2)
// ==========================================================
//增量式-M法码盘测速，原理如下：
//PinA:下降沿触发外部中断
//PinB:若A相下降沿触发,判断B相:若出现LOW电平,为正脉冲计数器加1
//                             若出现HIGH电平,为负脉冲计数器减1
//码盘统一成前进方向
// ==========================================================
#define PinA_left 18
#define PinB_left 22
#define PinA_right 19
#define PinB_right 23
int count_left;
int count_right;
//================================================
//===               Power_sample               ===
//================================================
#define FILTER_A 0.1
float Value_filter;
float Voltage;
char OK = 1;
//================================================
//===        UART_communication                ===
//================================================
byte  Buffer[3] = {0x00,0x00,0x00};
int integral = 0;
int decimal = 0;
                       //  0    1    2    3    4    5    6    7
byte Transmit_byte[8] = {0x0f,0x00,0x00,0x00,0x00,0x00,0x00,0x0f};
byte SBUF= 0x00;
int rec_flag = 0; //等于0等待接受 等于1正在接受
int index=0;
//=========================================================
//===                  PID_CONTROLLER                   ===
//=========================================================
int Pwm_out = 0;
int Turn_Need = 0;
int Speed_Need = 0;
float PWM_revise = 0;
int speed_output_R , speed_output_L;
float speeds , angle_filter, angle_i, w_filter, speeds_filter, positions,beta,a_beta;
float diff_speeds,diff_speeds_all,speed_diff_adjust ;
//=========================================================
float Kap = 23.2;
float Kai = 0.5;
float Kad = 9.58;
float Ksp = 2.25;
float Ksi = 0.102;
float K_speed_P = 2.2;
float K_speed_I = 1.5;
float filter_p = 0.3;
float filter_a = 0.9;
float filter_w = 0.4;
//=================================================
void PID_PWM()
{
  PWM_revise = 11.0 / Value_filter;

  speeds=(count_left + count_right)*0.5;

  speeds_filter *=(1 - filter_p);  //速度互补滤波防止抖动
  speeds_filter +=speeds*filter_p;

  if(fabs(speeds_filter)>=140)    //Using integration separation
    beta = 0.8;
  else
    beta = 1;

  angle_filter *=(1 - filter_a);    //角度互补滤波
  angle_filter +=Angle[0] * filter_a;

  if(fabs(angle_filter)>=9)    //Using integration separation
    a_beta = 0.5;
  else
    a_beta = 1;

  w_filter *=(1 - filter_w);      //角速度互补滤波
  w_filter +=w[0] * filter_w;

  positions += speeds_filter;
  positions += Speed_Need;
  positions = constrain(positions, -900, 1800);//抗积分饱和

  angle_i += angle_filter;
  angle_i = constrain(angle_i,-150,150);

  Pwm_out = Kap*angle_filter + a_beta*Kai*angle_i + Kad*w[0] + Ksp*speeds_filter + beta*Ksi*positions ;  //PID控制器
  Pwm_out *= PWM_revise;  //Power revise

  if(Turn_Need==0)
  {
    diff_speeds = count_right - count_left;
    diff_speeds_all += diff_speeds;
    diff_speeds_all = constrain(diff_speeds_all,-300,300);
    speed_diff_adjust = K_speed_P*diff_speeds+K_speed_I*diff_speeds_all;
  }
  else
    speed_diff_adjust = 0;

  speed_output_R = int(Pwm_out + Turn_Need -speed_diff_adjust);
  speed_output_L = int(Pwm_out - Turn_Need);

  Motor(speed_output_L , speed_output_R);
  
  count_left = 0;
  count_right = 0;
  
  Transmit_variable(0x01,angle_filter + 30);
  Transmit_variable(0x02,speeds_filter + 50);
  Transmit_variable(0x03,Value_filter);
  
  //Serial.println(angle_filter);  
  //Serial.print(count_right);   
  //Serial.print(',');
  //Serial.print(count_left); 
}
//================================================
//===                  Setup                   ===
//================================================

void setup() {
  analogReference(EXTERNAL);   //使用外部电压基准
  Serial.begin(115200);
  Serial.flush();
  Serial3.begin(115200);
  Serial3.flush();
  pinMode(M_left1, OUTPUT); //L298P直流电机驱动板的控制端口设置为输出模式
  pinMode(M_left2, OUTPUT);
  pinMode(M_right1, OUTPUT);
  pinMode(M_right2, OUTPUT);
  attachInterrupt(5, Code_left, FALLING);//小车左车轮编码器下降沿触发(外中断)
  attachInterrupt(4, Code_right, FALLING);
  delay(100);
}
//================================================
//===                  Loop                    ===
//================================================
//Loop time above: 2 ms~3 ms
//equal to adjust time :3 ms
//================================================
void loop()
{
  Remote();
  serialEvent();
  Power_sample();
  if(sign == 1)
  {
    sign=0;
    PID_PWM();
  }
}


